#include "HomographyFileParser.h"

#include <fstream> //for ifstream -> file input
#include <iostream> 
#include <sstream> //for stringstream
#include <string>
#include <deque>

#include <cv.h>

#include "../utils/Output.h"

using std::cout;
using std::endl;

HomographyFileParser::HomographyFileParser()
{
}

HomographyFileParser::~HomographyFileParser()
{
}




errorCodes::errorCode HomographyFileParser::parse(std::string inputFileName){

	std::ifstream inputFile(inputFileName.c_str());
	
	if(!inputFile.is_open()){
		Output::message("Could not open File %s!", Output::CRITICAL, inputFileName.c_str());
		return errorCodes::FAILURE;
	}
	
	std::string currentLine;
	
	std::string currentToken;
	
	
	std::deque<ImageParams> imageParams;

	std::getline(inputFile, currentLine);
	while(!inputFile.eof() && currentLine.find("# images") == std::string::npos){

		std::cout << currentLine << std::endl;
		
		
		std::getline(inputFile, currentLine);
	}
	std::cout << "found images" << std::endl;
	
	while(!inputFile.eof() && currentLine.find("# homographies") == std::string::npos){
		if (currentLine.find("#-hugin  cropFactor=1") != std::string::npos){
			cout << "disregarding " << currentLine << endl;
			//disregard
		} else if (currentLine.length() > 0 && currentLine.at(0) == 'i'){ // all image lines start with 'i'
			ImageParams currentParams;
			
			this->imageLines.push_back(currentLine);
			
			std::stringstream currentLineStream;
			currentLineStream << currentLine;
			cout << "processing line " << currentLineStream.str() << endl;
			while(std::getline(currentLineStream, currentToken, ' ')){ // get all whitespace-delimited items
				//std::cout << "item: |" << currentToken << "|" << std::endl;
				if (currentToken.length() > 0) {
				
					//let's make it some kind of state machine. It's a parser, after all.
					switch(currentToken.at(0)){
					case 'w':
						currentParams.w = atoi(currentToken.substr(1).c_str());
						break;
						
					case 'h':
						currentParams.h = atoi(currentToken.substr(1).c_str());
						break;
						
					case 'f':
						currentParams.f = atof(currentToken.substr(1).c_str());
						break;
						
					case 'E':
						switch(currentToken.at(1)){
						case 'b':
							currentParams.Eb = atof(currentToken.substr(2).c_str());
							break;
							
						case 'e':
							currentParams.Eev = atof(currentToken.substr(3).c_str());
							break;
							
						case 'r':
							currentParams.Er = atof(currentToken.substr(2).c_str());
							break;
						
						}
						break;
						
					case 'R':
						switch(currentToken.at(1)){
						case 'a':
							currentParams.Ra = atof(currentToken.substr(2).c_str());
							break;
							
						case 'b':
							currentParams.Rb = atof(currentToken.substr(2).c_str());
							break;
							
						case 'c':
							currentParams.Rc = atof(currentToken.substr(2).c_str());
							break;
							
						case 'd':
							currentParams.Rd = atof(currentToken.substr(2).c_str());
							break;
								
						case 'e':
							currentParams.Re = atof(currentToken.substr(2).c_str());
							break;
						}
						break;
						
					case 'V':
						switch(currentToken.at(1)){
						case 'a':
							currentParams.Va = atof(currentToken.substr(2).c_str());
							break;
							
						case 'b':
							currentParams.Vb = atof(currentToken.substr(2).c_str());
							break;
							
						case 'c':
							currentParams.Vc = atof(currentToken.substr(2).c_str());
							break;
							
						case 'd':
							currentParams.Vd = atof(currentToken.substr(2).c_str());
							break;
								
						case 'x':
							currentParams.Vx = atof(currentToken.substr(2).c_str());
							break;
							
						case 'y':
							currentParams.Vy = atof(currentToken.substr(2).c_str());
							break;
							
						case 'm':
							currentParams.Vm = atof(currentToken.substr(2).c_str());
							break;
						}
						break;
						
					case 'a':
						currentParams.a = atof(currentToken.substr(1).c_str());
						break;
						
					case 'b':
						currentParams.b = atof(currentToken.substr(1).c_str());
						break;
							
					case 'c':
						currentParams.c = atof(currentToken.substr(1).c_str());
						break;
						
					case 'd':
						currentParams.d = atof(currentToken.substr(1).c_str());
						break;
						
					case 'e':
						currentParams.e = atof(currentToken.substr(1).c_str());
						break;
						
					case 'g':
						currentParams.g = atof(currentToken.substr(1).c_str());
						break;
						
					case 'p':
						currentParams.p = atof(currentToken.substr(1).c_str());
						break;
						
					case 'r':
						currentParams.r = atof(currentToken.substr(1).c_str());
						break;
						
					case 't':
						currentParams.t = atof(currentToken.substr(1).c_str());
						break;
						
					case 'v':
						currentParams.v = atof(currentToken.substr(1).c_str());
						break;
						
					case 'y':
						currentParams.y = atof(currentToken.substr(1).c_str());
						break;
						
					case 'u':
						currentParams.u = atof(currentToken.substr(1).c_str());
						break;
						
					case 'n':
						currentParams.name = currentToken.substr(2, currentToken.length() -3 );
						Output::message("file name: %s", Output::DEBUG, currentToken.substr(2, currentToken.length() -3 ).c_str());
						break;
						
					}
				}
			}
			
			imageParams.push_back(currentParams);
			this->fileNames.push_back(currentParams.name);
			
		} else {
			//std::cout << "problem with line: " << currentLine << std::endl;
		}
		
		//cout << "image name: " << currentParams.name << endl;
		
		
		
		std::getline(inputFile, currentLine);
	}
	
	while(std::getline(inputFile, currentLine)){
		std::stringstream currentLineStream;
			
		currentLineStream.str(currentLine);

		
		
		if(currentLine == ""){
			continue;
		}
		
		/* parse image indices */
		if (currentLine.find("images") != std::string::npos){
			Homography currentHomography;
			
			Output::message("Reading IDs", Output::DEBUG);
			
			/* there must be three tokens - "images", left index, right index */
			/* skip the first one */ 
			std::getline(currentLineStream, currentToken, ' '); // get all whitespace-delimited items
			
			/* get left image ID */
			std::getline(currentLineStream, currentToken, ' ');		
			currentHomography.leftImageID = atoi(currentToken.c_str());
			
			/* get right image ID */
			std::getline(currentLineStream, currentToken, ' ');
			currentHomography.rightImageID = atoi(currentToken.c_str());
			
			this->homographies.push_back(currentHomography);
			
		} else {
			
			Output::message("Reading homography");
			
			float currentValue;
			/* there are three whitespace-delimited lines */
			std::getline(currentLineStream, currentToken, ' ');
			currentValue = atof(currentToken.c_str());
			Output::message("Value: %f", Output::DEBUG, currentValue);
			cvmSet(this->homographies.back().data, 0, 0, currentValue);
			
			std::getline(currentLineStream, currentToken, ' ');
			currentValue = atof(currentToken.c_str());
			Output::message("Value: %f", Output::DEBUG, currentValue);
			cvmSet(this->homographies.back().data, 0, 1, currentValue);
			
			std::getline(currentLineStream, currentToken, ' ');
			currentValue = atof(currentToken.c_str());
			Output::message("Value: %f", Output::DEBUG, currentValue);
			cvmSet(this->homographies.back().data, 0, 2, currentValue);
			
			/* get the next line */
			std::getline(inputFile, currentLine);
			
			currentLineStream.str("");
			currentLineStream.clear();
			currentLineStream << currentLine;
			
			std::getline(currentLineStream, currentToken, ' ');
			currentValue = atof(currentToken.c_str());
			Output::message("Value: %f", Output::DEBUG, currentValue);
			cvmSet(this->homographies.back().data, 1, 0, currentValue);
			
			std::getline(currentLineStream, currentToken, ' ');
			currentValue = atof(currentToken.c_str());
			Output::message("Value: %f", Output::DEBUG, currentValue);
			cvmSet(this->homographies.back().data, 1, 1, currentValue);
			
			std::getline(currentLineStream, currentToken, ' ');
			currentValue = atof(currentToken.c_str());
			Output::message("Value: %f", Output::DEBUG, currentValue);
			cvmSet(this->homographies.back().data, 1, 2, currentValue);
			
			/* get the next line */
			std::getline(inputFile, currentLine);
			
			currentLineStream.str("");
			currentLineStream.clear();
			currentLineStream << currentLine;
			
			std::getline(currentLineStream, currentToken, ' ');
			currentValue = atof(currentToken.c_str());
			Output::message("Value: %f", Output::DEBUG, currentValue);
			cvmSet(this->homographies.back().data, 2, 0, currentValue);
			
			std::getline(currentLineStream, currentToken, ' ');
			currentValue = atof(currentToken.c_str());
			Output::message("Value: %f", Output::DEBUG, currentValue);
			cvmSet(this->homographies.back().data, 2, 1, currentValue);
			
			std::getline(currentLineStream, currentToken, ' ');
			currentValue = atof(currentToken.c_str());
			Output::message("Value: %f", Output::DEBUG, currentValue);
			cvmSet(this->homographies.back().data, 2, 2, currentValue);
			
			
			
		}
	}
	
	Output::message("Parsed homographies: ", Output::DEBUG);
	for (unsigned int i = 0; i < homographies.size(); ++i) {
		Output::message("IDs: %d, %d", Output::DEBUG, homographies[i].leftImageID, homographies[i].rightImageID);
		Output::message("%f %f %f", Output::DEBUG, cvmGet(homographies[i].data, 0, 0), cvmGet(homographies[i].data, 0, 1), cvmGet(homographies[i].data, 0, 2));
		Output::message("%f %f %f", Output::DEBUG, cvmGet(homographies[i].data, 1, 0), cvmGet(homographies[i].data, 1, 1), cvmGet(homographies[i].data, 1, 2));
		Output::message("%f %f %f", Output::DEBUG, cvmGet(homographies[i].data, 2, 0), cvmGet(homographies[i].data, 2, 1), cvmGet(homographies[i].data, 2, 2));
		Output::separator("");
	}
	
	
//	Output::message("Parsed feature pairs: ", Output::DEBUG);
//	for (unsigned int i = 0; i < featurePairs.size(); ++i) {
//		Output::message("(%f, %f) -> (%f, %f), id: %010llu", Output::DEBUG, featurePairs[i].left.x, featurePairs[i].left.y, featurePairs[i].right.x, featurePairs[i].right.y, featurePairs[i].id);
//	}
		
	return errorCodes::SUCCESS;
}
	
std::deque<Homography> HomographyFileParser::getHomographies(){
	return this->homographies;
}

std::deque<std::string> HomographyFileParser::getFileNames(){
	return this->fileNames;
}

std::deque<std::string> HomographyFileParser::getImageLines(){
	return this->imageLines;
}

